#include <30F5015.h>
#FUSES NOWDT                    //No Watch Dog Timer
#FUSES FRC_PLL16                //Internal Fast RC oscillator with 4X PLL
#FUSES PR_PLL                   //Primary Oscillator with PLL
#FUSES NOCKSFSM                 //Clock Switching is disabled, fail Safe clock monitor is disabled
#FUSES WPSB16                   //Watch Dog Timer PreScalar B 1:16
#FUSES WPSA512                  //Watch Dog Timer PreScalar A 1:512
#FUSES PUT64                    //Power On Reset Timer value 64ms
#FUSES NOBROWNOUT               //No brownout reset
#FUSES BORRES                
#FUSES LPOL_HIGH                //Low-Side Transistors Polarity is Active-High (PWM 0,2,4 and 6)
   //PWM module low side output pins have active high output polar
#FUSES HPOL_HIGH                //High-Side Transistors Polarity is Active-High (PWM 1,3,5 and 7)
   //PWM module high side output pins have active high output polarity
#FUSES NOPWMPIN                 //PWM outputs drive active state upon Reset
#FUSES MCLR                     //Master Clear pin enabled
#FUSES NOPROTECT                //Code not protected from reading
#FUSES NOWRT                    //Program memory not write protected
#FUSES NODEBUG                  //No Debug mode for ICD
#FUSES NOCOE                    //Device will reset into operational mode
//#FUSES ICSP1                    //ICD uses PGC1/PGD1 pins
#use delay(clock=117920000)
#use rs232(UART1,baud=921600,parity=N,bits=8)

#byte TRISB0 =0x2C6
#byte TRISB1 =0x2C7
#byte PORTB0 =0x2C8
#byte PORTB1 =0x2C9

#include "Global_Variables.c"
#include "Functions.h"

void main(void)
{
	TRISB0=0xFF;    //Configuration for PORTB as input B0-B7
	TRISB1=0xFF;	//Configuration for PORTB as input B8-B15
	
   	setup_adc_ports(sAN0|sAN1|sAN2|sAN3|sAN4|sAN5|sAN6|sAN7|sAN8|sAN9|sAN10|sAN11|sAN12|sAN13|sAN14|sAN15);//*******Setup ch0 and ch1 as ADC inputs in Px and Py******
   	setup_adc(ADC_CLOCK_INTERNAL);

	//Configuracion PWM 915hz
 	setup_compare(1, COMPARE_PWM | COMPARE_TIMER3); //en OC1 pin 46 M1    --      
 	setup_compare(2, COMPARE_PWM | COMPARE_TIMER2); // en OC2 pin 49 M1
 
   	setup_timer2(TMR_INTERNAL | TMR_DIV_BY_64,499); 
   	setup_timer3(TMR_INTERNAL | TMR_DIV_BY_64,499);

   	set_pwm_duty(1, 0); 	// M2 Duty 0%   LEFT+
   	set_pwm_duty(2, 0); 	// M1 Duty 0% 	LEFT-

	while(!input_state(PIN_D9))	//While loop used for waiting an input high  in D8 to start
	{
		output_toggle(PIN_D4);
		delay_ms(250);
	}
	output_high(PIN_D4);
	delay_ms(2000);
    while(true)
	{
		ReadSensors(Sensors_ADC);
		DigitalizeArray(Sensors_ADC,Sensors_BIN,Black_Limit,0,14);
		GetAverageCenter(Sensors_BIN,0,14,&Center);
		
		if(Center==-1)	{ output_low(PIN_D4);output_low(PIN_D6);Center=Last_Center; if(KD<2*KP) KD=KD+20;}//output_high(PIN_D5);}
		if(Center<8 && Center>=0)	{ output_high(PIN_D6);output_low(PIN_D4);KD=0;}//output_low(PIN_D5);}
		if(Center>8)	{ output_high(PIN_D4);output_low(PIN_D6);KD=0;}//output_low(PIN_D5);}
		if(Center==8)	{ output_low(PIN_D4);output_low(PIN_D6);KD=0;}//output_low(PIN_D5);}
						
		Last_Center=Center;		
		error=8-Center;
		correction=(error*KP);
		//PoweredError(error,&correction,5,BaseSpeed);
		Left_Correction=BaseSpeed-correction;
		Right_Correction=BaseSpeed+correction;
				
		if(Left_Correction<0)		 Left_correction=0;
		if(Left_Correction>499)		 Left_correction=499;
		if(Right_Correction<0)	 Right_correction=0;
		if(Right_Correction>499)	 Right_correction=499;

		set_pwm_duty(1, (int16)Left_correction); // 
		set_pwm_duty(2, (int16)Right_Correction); // 		
	}
}